package com.itistimetothink.cjetso.utilities;

import java.util.List;
import java.util.concurrent.atomic.AtomicBoolean;

import com.google.android.maps.MapActivity;

import android.app.Activity;
import android.content.Context;
import android.hardware.GeomagneticField;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.os.Bundle;
import android.util.Log;

public class SensorsActivity extends MapActivity implements SensorEventListener, LocationListener {
    private static final String TAG = "SensorsActivity";
    private static final AtomicBoolean computing = new AtomicBoolean(false); 
    private static boolean initState = true;
    public static final float EPSILON = 0.000000001f;
    private static final float NS2S = 1.0f / 1000000000.0f;
	private float timestamp = 0.0f;

    private static final int MIN_TIME = 15*1000;
    private static final int MIN_DISTANCE = 10;

    private static final float temp[] = new float[9];
    private static final float rotation[] = new float[9];
    private static final float grav[] = new float[3];
    private static final float mag[] = new float[3];
    
	// orientation angles from gyro matrix
    private static float gyroOrientation[] = new float[3];
	// orientation angles from accel and magnet
	private float[] accMagOrientation = new float[3];
	// final orientation angles from sensor fusion
	private float[] fusedOrientation = new float[3];
    
    private static SensorFusion sensorFusion = new SensorFusion();
    private static final Matrix gyroMatrix = new Matrix();    
    //private float[] gyroMatrix = new float[9];   
    private static final Matrix worldCoord = new Matrix();
    private static final Matrix magneticCompensatedCoord = new Matrix();
    private static final Matrix xAxisRotation = new Matrix();
    private static final Matrix magneticNorthCompensation = new Matrix();
    
    private static GeomagneticField gmf = null;
    private static float smooth[] = new float[3];
    private static SensorManager sensorMgr = null;
    private static List<Sensor> sensors = null;
    private static Sensor sensorGrav = null;
    private static Sensor sensorMag = null;
    private static Sensor sensorGyro = null;

    private static LocationManager locationMgr = null;

	@Override
    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
    }

	@Override
    public void onStart() {
        super.onStart();

        double angleX = Math.toRadians(-90);
        double angleY = Math.toRadians(-90);
        xAxisRotation.set( 1f, 
                0f, 
                0f, 
                0f, 
                (float) Math.cos(angleX), 
                (float) -Math.sin(angleX), 
                0f, 
                (float) Math.sin(angleX), 
                (float) Math.cos(angleX));

        try {
            sensorMgr = (SensorManager) getSystemService(SENSOR_SERVICE);

            sensors = sensorMgr.getSensorList(Sensor.TYPE_ACCELEROMETER);
            
            if (sensors.size() > 0) 
            {
            	sensorGrav = sensors.get(0);
            }
            
            sensors = sensorMgr.getSensorList(Sensor.TYPE_MAGNETIC_FIELD);
            
            if (sensors.size() > 0) 
            {
            	sensorMag = sensors.get(0);
            }
            sensors = sensorMgr.getSensorList(Sensor.TYPE_GYROSCOPE);
            
            if (sensors.size() > 0) 
            {
            	sensorGyro = sensors.get(0);
            }
            sensorMgr.registerListener(this, sensorGrav, SensorManager.SENSOR_DELAY_FASTEST );
            sensorMgr.registerListener(this, sensorGyro, SensorManager.SENSOR_DELAY_FASTEST );
            sensorMgr.registerListener(this, sensorMag, SensorManager.SENSOR_DELAY_FASTEST );

            locationMgr = (LocationManager) getSystemService(Context.LOCATION_SERVICE);
            locationMgr.requestLocationUpdates(LocationManager.NETWORK_PROVIDER, MIN_TIME, MIN_DISTANCE, this);

            try {

                try {
                    Location gps=locationMgr.getLastKnownLocation(LocationManager.GPS_PROVIDER);
                    Location network=locationMgr.getLastKnownLocation(LocationManager.NETWORK_PROVIDER);
                    if(gps!=null)
                    {
                        onLocationChanged(gps);
                    }
                    else if (network!=null)
                    { 
                        onLocationChanged(network);
                    }
                    else
                    {
                        onLocationChanged(ARData.hardFix);
                    }
                } catch (Exception ex2) {
                    onLocationChanged(ARData.hardFix);
                }

                gmf = new GeomagneticField((float) ARData.getCurrentLocation().getLatitude(), 
                                           (float) ARData.getCurrentLocation().getLongitude(),
                                           (float) ARData.getCurrentLocation().getAltitude(), 
                                           System.currentTimeMillis());
                angleY = Math.toRadians(-gmf.getDeclination());

                synchronized (magneticNorthCompensation) {

                    magneticNorthCompensation.toIdentity();

                    magneticNorthCompensation.set( (float) Math.cos(angleY), 
                                                  0f, 
                                                  (float) Math.sin(angleY), 
                                                  0f, 
                                                  1f, 
                                                  0f, 
                                                  (float) -Math.sin(angleY), 
                                                  0f, 
                                                  (float) Math.cos(angleY));
    
                    magneticNorthCompensation.prod(xAxisRotation);
                }
            } catch (Exception ex) {
            	ex.printStackTrace();
            }
        } catch (Exception ex1) {
            try {
                if (sensorMgr != null) {
                    sensorMgr.unregisterListener(this, sensorGrav);
                    sensorMgr.unregisterListener(this, sensorMag);
                    sensorMgr.unregisterListener(this, sensorGyro);
                    sensorMgr = null;
                }
                if (locationMgr != null) {
                    locationMgr.removeUpdates(this);
                    locationMgr = null;
                }
            } catch (Exception ex2) {
            	ex2.printStackTrace();
            }
        }
    }

	@Override
    protected void onStop() {
        super.onStop();

        try {

                try {
                	if(sensorMgr != null){
                    sensorMgr.unregisterListener(this, sensorGrav);
                    sensorMgr.unregisterListener(this, sensorMag);
                    sensorMgr.unregisterListener(this, sensorGyro);
                	}
                } catch (Exception ex) {
                	ex.printStackTrace();
                }
        	

            sensorMgr = null;

            try {
            	if(locationMgr != null){
                    locationMgr.removeUpdates(this);
            	}
            } catch (Exception ex) {
            	ex.printStackTrace();
            }
            locationMgr = null;
        } catch (Exception ex) {
        	ex.printStackTrace();
        }
    }

    public void onSensorChanged(SensorEvent evt) {
    	if (!computing.compareAndSet(false, true)) return;

    	
        /**
         * Newly added
         */
        if (evt.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
        	//TODO do something

        	float [] gyro = new float [3];
            if (SensorManager.getRotationMatrix(temp, null, grav, mag)){
            	gyro = evt.values;
            	//Log.i("gyrpscope", "gyro value: "+gyro[0]);
            	//Log.i("gyrpscope", "gyro value: "+gyro[1]);
            	//Log.i("gyrpscope", "gyro value: "+gyro[2]);
            	// added on 12-1-2013
            	//
            	float [] accMagOrientation = new float [3];
    	        SensorManager.getOrientation(temp, accMagOrientation);
    	        if(initState) {
                	gyroMatrix.toIdentity();
    	        	float [] initMatrix = new float [9];
    	        	initMatrix = temp;
    	        	Matrix tempMatrix = new Matrix();
    	        	tempMatrix.set(initMatrix[0], initMatrix[1], initMatrix[2], initMatrix[3], 
    	        			initMatrix[4], initMatrix[5], initMatrix[6], initMatrix[7], initMatrix[8]);
                   
     	        	gyroMatrix.prod(tempMatrix);
    	            initState = false;
    	            float [] array = new float [9];
    	            gyroMatrix.get(array);
                   
    	            //gyroMatrix initialization
    	        }
    	        
    	        float[] deltaVector = new float[4];
    	        if(timestamp != 0) {
    	            final float dT = (evt.timestamp - timestamp) * NS2S;
    	        //System.arraycopy(evt.values, 0, gyro, 0, 3);
    	        getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
    	        }
    	     
    	        // measurement done, save current time for next interval
    	        timestamp = evt.timestamp;
    	     
    	        // convert rotation vector into rotation matrix
    	        float[] deltaMatrix = new float[9];
    	        getRotationMatrixFromVector(deltaMatrix, deltaVector);
    	        Matrix realDeltaMatrix = new Matrix();
               
    	        realDeltaMatrix.set(deltaMatrix[0], deltaMatrix[1], deltaMatrix[2], 
    	        		            deltaMatrix[3], deltaMatrix[4], deltaMatrix[5], 
    	        		            deltaMatrix[6], deltaMatrix[7], deltaMatrix[8]);
    	        gyroMatrix.prod(realDeltaMatrix);    	        
    	        float [] array = new float[9];
    	        gyroMatrix.get(array);
                
    	        SensorManager.getOrientation(array, gyroOrientation);
    	        // newly added
    	        fusedOrientation = sensorFusion.calculateFusedOrientationTask(gyroOrientation, accMagOrientation);
    	        sensorFusion.getRotationMatrixFromOrientation(fusedOrientation, temp);
    	    	 // newly added
            }
        	
        }
        
        if (evt.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            //smooth = LowPassFilter.filter(0.5f, 1.0f, evt.values, grav);
        	smooth = LowPassFilter.filter(0.5f, 1.0f, evt.values, grav);
            grav[0] = smooth[0];
            grav[1] = smooth[1];
            grav[2] = smooth[2];
            SensorManager.getRotationMatrix(temp, null, grav, mag);

        } else if (evt.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
            smooth = LowPassFilter.filter(2.0f, 4.0f, evt.values, mag);
            mag[0] = smooth[0];
            mag[1] = smooth[1];
            mag[2] = smooth[2];
            SensorManager.getRotationMatrix(temp, null, grav, mag);

        }

        
        //SensorManager.getRotationMatrix(temp, null, grav, mag);
        SensorManager.getOrientation(temp, accMagOrientation);
        

    	
        SensorManager.remapCoordinateSystem(temp, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, rotation);

        worldCoord.set(rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6], rotation[7], rotation[8]);

        magneticCompensatedCoord.toIdentity();

        synchronized (magneticNorthCompensation) {
            magneticCompensatedCoord.prod(magneticNorthCompensation);
        }

        magneticCompensatedCoord.prod(worldCoord);

        magneticCompensatedCoord.invert(); 

        ARData.setRotationMatrix(magneticCompensatedCoord);

        computing.set(false);
    }

    public void onProviderDisabled(String provider) {
        //Not Used
    }
	
    public void onProviderEnabled(String provider) {
        //Not Used
    }

    public void onStatusChanged(String provider, int status, Bundle extras) {
        //Not Used
    }

    public void onLocationChanged(Location location) {
        ARData.setCurrentLocation(location);
        gmf = new GeomagneticField((float) ARData.getCurrentLocation().getLatitude(), 
                (float) ARData.getCurrentLocation().getLongitude(),
                (float) ARData.getCurrentLocation().getAltitude(), 
                System.currentTimeMillis());
        
        double angleY = Math.toRadians(-gmf.getDeclination());

        synchronized (magneticNorthCompensation) {
            magneticNorthCompensation.toIdentity();
            
            magneticNorthCompensation.set((float) Math.cos(angleY), 
                                         0f, 
                                         (float) Math.sin(angleY), 
                                         0f, 
                                         1f, 
                                         0f, 
                                         (float) -Math.sin(angleY), 
                                         0f, 
                                         (float) Math.cos(angleY));
    
            magneticNorthCompensation.prod(xAxisRotation);
        }
    }

    public void onAccuracyChanged(Sensor sensor, int accuracy) {
		if (sensor==null) throw new NullPointerException();
		
        if(sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD && accuracy==SensorManager.SENSOR_STATUS_UNRELIABLE) {
//            Log.e(TAG, "Compass data unreliable");
        }
    }
    
	// This function is borrowed from the Android reference
	// at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
	// It calculates a rotation vector from the gyroscope angular speed values.
    private void getRotationVectorFromGyro(float[] gyroValues,
            float[] deltaRotationVector,
            float timeFactor)
	{
		float[] normValues = new float[3];
		
		// Calculate the angular speed of the sample
		float omegaMagnitude =
		(float)Math.sqrt(gyroValues[0] * gyroValues[0] +
		gyroValues[1] * gyroValues[1] +
		gyroValues[2] * gyroValues[2]);
		
		// Normalize the rotation vector if it's big enough to get the axis
		if(omegaMagnitude > EPSILON) {
		normValues[0] = gyroValues[0] / omegaMagnitude;
		normValues[1] = gyroValues[1] / omegaMagnitude;
		normValues[2] = gyroValues[2] / omegaMagnitude;
		}
		
		// Integrate around this axis with the angular speed by the timestep
		// in order to get a delta rotation from this sample over the timestep
		// We will convert this axis-angle representation of the delta rotation
		// into a quaternion before turning it into the rotation matrix.
		float thetaOverTwo = omegaMagnitude * timeFactor;
		float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
		float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
		deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
		deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
		deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
		deltaRotationVector[3] = cosThetaOverTwo;
	}
    
    /** Helper function to convert a rotation vector to a rotation matrix.
     *  Given a rotation vector (presumably from a ROTATION_VECTOR sensor), returns a
     *  9  or 16 element rotation matrix in the array R.  R must have length 9 or 16.
     *  If R.length == 9, the following matrix is returned:
     * <pre>
     *   /  R[ 0]   R[ 1]   R[ 2]   \
     *   |  R[ 3]   R[ 4]   R[ 5]   |
     *   \  R[ 6]   R[ 7]   R[ 8]   /
     *</pre>
     * If R.length == 16, the following matrix is returned:
     * <pre>
     *   /  R[ 0]   R[ 1]   R[ 2]   0  \
     *   |  R[ 4]   R[ 5]   R[ 6]   0  |
     *   |  R[ 8]   R[ 9]   R[10]   0  |
     *   \  0       0       0       1  /
     *</pre>
     *  @param rotationVector the rotation vector to convert
     *  @param R an array of floats in which to store the rotation matrix
     */
    public static void getRotationMatrixFromVector(float[] R, float[] rotationVector) {
        float q0 = (float)Math.sqrt(1 - rotationVector[0]*rotationVector[0] -
                                    rotationVector[1]*rotationVector[1] -
                                    rotationVector[2]*rotationVector[2]);
        float q1 = rotationVector[0];
        float q2 = rotationVector[1];
        float q3 = rotationVector[2];

        float sq_q1 = 2 * q1 * q1;
        float sq_q2 = 2 * q2 * q2;
        float sq_q3 = 2 * q3 * q3;
        float q1_q2 = 2 * q1 * q2;
        float q3_q0 = 2 * q3 * q0;
        float q1_q3 = 2 * q1 * q3;
        float q2_q0 = 2 * q2 * q0;
        float q2_q3 = 2 * q2 * q3;
        float q1_q0 = 2 * q1 * q0;

        if(R.length == 9) {
            R[0] = 1 - sq_q2 - sq_q3;
            R[1] = q1_q2 - q3_q0;
            R[2] = q1_q3 + q2_q0;

            R[3] = q1_q2 + q3_q0;
            R[4] = 1 - sq_q1 - sq_q3;
            R[5] = q2_q3 - q1_q0;

            R[6] = q1_q3 - q2_q0;
            R[7] = q2_q3 + q1_q0;
            R[8] = 1 - sq_q1 - sq_q2;
        } else if (R.length == 16) {
            R[0] = 1 - sq_q2 - sq_q3;
            R[1] = q1_q2 - q3_q0;
            R[2] = q1_q3 + q2_q0;
            R[3] = 0.0f;

            R[4] = q1_q2 + q3_q0;
            R[5] = 1 - sq_q1 - sq_q3;
            R[6] = q2_q3 - q1_q0;
            R[7] = 0.0f;

            R[8] = q1_q3 - q2_q0;
            R[9] = q2_q3 + q1_q0;
            R[10] = 1 - sq_q1 - sq_q2;
            R[11] = 0.0f;

            R[12] = R[13] = R[14] = 0.0f;
            R[15] = 1.0f;
        }
    }

	@Override
	protected boolean isRouteDisplayed() {
		// TODO Auto-generated method stub
		return false;
	}

}